%% 混合A*算法测试脚本
clc;
cla;

% 读取地图
load('maps/carparking_map.mat','mapdata');
GlobalMap = mapdata';

% 初始化参数
ObstTolerance = 0.2;
CarWidth = 2;
CarLength = 3;
Start = [10, 20, 1.57];
End = [26.5, 6];
SatLevel = [1, 0.5];

% 混合A*搜索
[x,y,theta,D,delta,totalDistance] = SearchPath(ObstTolerance,GlobalMap, ...
    Start,End,CarWidth,CarLength,SatLevel,1,30*60);
if isempty(x)
    disp('Failed to find path!')
end